package behavior;

import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class NoLeftWall implements Behavior {
	private UltrasonicSensor sonar;
	private DifferentialPilot pilot = new DifferentialPilot(28f, 105f, Motor.C, Motor.A, false);
	private boolean suppressed = false;

	public NoLeftWall(SensorPort port)
	{
		sonar = new UltrasonicSensor(port);
	}

	public boolean takeControl() {
		try {
			Thread.sleep(50);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}

		return ((sonar.getDistance() > 50) && (Motor.B.getTachoCount() < -5));
	}

	public void suppress() {
		suppressed = true;
	}

	public void action() {
		suppressed = false;
		LCD.clear();
		pilot.setTravelSpeed(50);
		Motor.B.rotateTo(0, true);

		while( Motor.B.isMoving() && !suppressed )
			Thread.yield();

		Motor.B.stop();
		
		if(Motor.B.getTachoCount() != 0){
			while(Motor.B.getTachoCount() != 0){
				Motor.B.rotateTo(0, true);

				while( Motor.B.isMoving() && !suppressed )
					Thread.yield();
			}
			Motor.B.stop();
		}
		
		Motor.B.resetTachoCount();

		LCD.drawString("Turn left", 0, 2);
		pilot.setRotateSpeed(45);
		pilot.rotate(45, true);
		
		while(pilot.isMoving() && !suppressed)
			Thread.yield();  // wait till turn is complete or suppressed is called

		pilot.stop();
	}
}